/*
* @File Name AStar.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\route\realTimePrm\AStar.c
* @Author: Ruige_Lee
* @Date:   2019-08-22 19:38:57
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-09-26 20:08:28
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/


#include <stdint.h>
#include <string.h>


#include "prm.h"



static int32_t Astar_execute( float staPose[6], float endPose[6], float* routeIndex[2*MAX_CHECKPOINT], uint8_t iteration );
static inline int32_t AStar_value_cal(float staPose[6], float endPose[6], float value[SAMPLETIMES]);
static inline int32_t AStar_value_sort(float value[SAMPLETIMES], uint32_t poseValueIndex[SAMPLETIMES]);
static int32_t AStar_next_pose( float staPose[6], float endPose[6], uint32_t poseeValueIndex[SAMPLETIMES], float* routeIndex[2*MAX_CHECKPOINT], uint8_t iteration );
static inline int32_t AStar_init_route( float* routeIndex[2*MAX_CHECKPOINT] );
static inline int32_t AStar_load_route(float* pose_A6, float* pose_B6, float* routeIndex[2*MAX_CHECKPOINT]);








int32_t prm_Astar_top( float staPose[6], float endPose[6], float* routeIndex[2 * MAX_CHECKPOINT] )
{
	int32_t status = 0;

	AStar_init_route( &routeIndex[0] );


	status = Astar_execute( &staPose[0], &endPose[0], &routeIndex[0], ITERATION );

	return status;
}

static int32_t Astar_execute( float staPose[6], float endPose[6], float* routeIndex[2*MAX_CHECKPOINT], uint8_t iteration )
{
	int32_t status;
	uint8_t iter = iteration - 1;


	if ( 0 == prm_direct_check( &staPose[0], &endPose[0], CHECKSTEP ) )
	{
		//load route
		//如果可以直达，装填路径，立即返回
		AStar_load_route(staPose, endPose, routeIndex);
		return 0;
	}

	uint32_t poseValueIndex[SAMPLETIMES];
	float value[SAMPLETIMES] = {0};



	//计算价值
	AStar_value_cal( &staPose[0], &endPose[0], &value[0] );

	//价值排序,只需要排出前10就行
	AStar_value_sort( &value[0], &poseValueIndex[0] );


	//查看当前节点库里有没有单点联通，没有的话，看下一个双点
	status = AStar_next_pose( &staPose[0], &endPose[0], &poseValueIndex[0], &routeIndex[0], iter);

	if ( status == 0 )
	{
		AStar_load_route(staPose, endPose, routeIndex);
	}

	return status;
}



static inline int32_t AStar_value_cal(float staPose[6], float endPose[6], float value[SAMPLETIMES])
{

	//计算价值
	for ( uint32_t i = 0; i < SAMPLETIMES; i++ )
	{
		float valueSum = 0.00;
		for ( uint8_t j = 0; j < 3; j++ ) //只计算前三轴的价值
		{
			valueSum += fast_abs(samplePose[i][j]-staPose[j])
			 				+ fast_abs(samplePose[i][j]-endPose[j]);
		}
		value[i] = valueSum;
	}
	return 0;
}


static inline int32_t AStar_value_sort(float value[SAMPLETIMES], uint32_t poseValueIndex[SAMPLETIMES])
{
	//排序之前，指针初始化
	for ( uint32_t i = 0; i < SAMPLETIMES; i++ )
	{
		poseValueIndex[i] = i;
	}

	//价值排序,只需要排出前30就行
	for ( uint32_t i = 0; i < 30; i++ )
	{
		for ( uint32_t j = (SAMPLETIMES - 1); j > i; j -- )
		{
			if ( value[j] > value[j-1] )
			{
				;
			}
			else
			{
				float valueTemp;
				uint32_t valueIndexTemp;

				valueTemp = value[j];
				value[j] = value[j-1];
				value[j-1] = valueTemp;

				valueIndexTemp = poseValueIndex[j];
				poseValueIndex[j] = poseValueIndex[j-1];
				poseValueIndex[j-1] = valueIndexTemp;
			}
		}
	}
	return 0;
}


static int32_t AStar_next_pose( float staPose[6], float endPose[6], uint32_t poseValueIndex[SAMPLETIMES], float* routeIndex[2*MAX_CHECKPOINT], uint8_t iteration )
{
	//查看当前节点库里有没有单点联通，没有的话，看下一个双点
	uint8_t iter = iteration;
	uint32_t staPoseNearByIndex[10];
	uint8_t staNearByCnt = 0;
	uint32_t endPoseNearByIndex[10];
	uint8_t endNearByCnt = 0;

	//取损耗最低的前10
	//遍历看是否单点双连通，记录单点单连通的点
	for ( uint32_t i = 15; i < 25; i++ )
	{	
		uint32_t index = poseValueIndex[i];
		int32_t status_A, status_B;

		status_A = prm_direct_check( &staPose[0], &samplePose[index][0], CHECKSTEP );
		status_B = prm_direct_check( &endPose[0], &samplePose[index][0], CHECKSTEP );

		if ( (status_A == 0) && (status_B == 0) )
		{
			//单点直通
			AStar_load_route( &samplePose[index][0], NULL, &routeIndex[0] );
			return 0;
		}
		else
		{
			if ( status_A == 0 )
			{
				staPoseNearByIndex[staNearByCnt] = index;
				staNearByCnt ++;
			}
			if ( status_B == 0 )
			{
				endPoseNearByIndex[endNearByCnt] = index;
				endNearByCnt ++;
			}
		}
	}

	if ( iter == 0 || staNearByCnt == 0 || endNearByCnt == 0 )
	{
		//不能直达，但是已经没有迭代次数了
		//在其中一个点附近就没有节点
		return -1;
	}

	//遍历递归两个单连通点集合作为起点终点
	for ( uint32_t i = 0 ; i < staNearByCnt; i++ )
	{
		for ( uint32_t j = 0; j < endNearByCnt; j++ )
		{
			uint32_t staIndex = staPoseNearByIndex[i];
			uint32_t endIndex = endPoseNearByIndex[j];
			int32_t status = 0;
			status = Astar_execute( &samplePose[staIndex][0], &samplePose[endIndex][0], &routeIndex[0], iter );
			if ( status == -1 )
			{
				//没有找到，可能是因为内层递归耗尽
				
				//检查还有没有碰撞检测次数
				continue;
			}
			if ( status == 0 )
			{
				//找到了，返回，内层已经装载了，这里不重复装载
				//AStar_load_route( NULL, NULL, &routeIndex[0] );
				return 0;
			}
		}
	}
	return -1;
}



static inline int32_t AStar_init_route( float* routeIndex[2*MAX_CHECKPOINT] )
{
	for ( uint32_t i = 0; i < ( 2 * MAX_CHECKPOINT ); i ++ )
	{
		routeIndex[i] = NULL;
	}
	return 0;
}


static inline int32_t AStar_load_route(float* pose_A6, float* pose_B6, float* routeIndex[2*MAX_CHECKPOINT])
{
	for ( int32_t i = (MAX_CHECKPOINT - 1); i >= 0; i-- )
	{
		if ( routeIndex[i] != NULL )
		{
			continue;
		}
		else
		{
			routeIndex[i] = pose_A6;
			break;
		}
	}

	for ( int32_t i = MAX_CHECKPOINT; i < ( 2 * MAX_CHECKPOINT ); i++ )
	{
		if ( routeIndex[i] != NULL )
		{
			continue;
		}
		else
		{
			if ( pose_B6 != NULL )
			{
				routeIndex[i] = pose_B6;
				break;
			}
		}
	}
	return 0;
}
